package src.bojan;

import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import sim.field.continuous.Continuous2D;
import sim.util.Bag;
import sim.util.Double2D;
import sim.util.Int2D;
import bojan.robots.simulation.Obstacle;

public class TerrainMap {

	Continuous2D grid;

	public TerrainMap(Continuous2D grid) {
		super();
		this.grid = grid;
	}

	public boolean isUnknown(Double2D point) {
		return mapping(point) instanceof Unknown;
	}

	public boolean isObstacle(Double2D npe) {
		return mapping(npe) instanceof Obstacle;
	}

	

	public Mapping mapping(Double2D npe) {
		Int2D np = grid.discretize(npe);
		Mapping type = null;
		if (grid.getObjectsAtLocation(np) != null) {
			for (Object obj : grid.getObjectsAtLocation(np)) {
				if (obj instanceof Mapping) {
					if (type == null) {
						type = (Mapping) obj;
					} else {
						if (!type.getClass().equals(obj.getClass())) {
							throw new RuntimeException(
									"More than one mapping at cell " + npe);
						}
					}
				}
			}
		}
		return type;
	}

	public int visitedInZone(Double2D point, double radius) {
		Set<Mapping> objs = getSurrounding(point, radius);
		int visited = 0;
		for (Mapping object : objs) {		
			if (object instanceof Known) {
				visited ++;
			}
		}
		return visited;
	}

	public int unknownInZone(Double2D point, double radius) {
		Set<Mapping> objs = getSurrounding(point, radius);
		int unknown = 0;
		for (Mapping object : objs) {		
			if (object instanceof Unknown) {
				unknown ++;
			}
		}
		 return unknown;
	}
	
	private Set<Mapping> getSurrounding(Double2D point, double radius) {
		HashSet<Mapping> a = new HashSet<Mapping>();
		Bag b = grid.getObjectsExactlyWithinDistance(point, radius);
		for (Object object : b) {
			if (object instanceof Mapping) { 
				a.add((Mapping)object);
			}
		}
		return a;
	}

	public void map(Double2D po, Known mapping) {
		Mapping currentMapping = mapping(po);
		if (currentMapping instanceof Known) {
			throw new RuntimeException("Position " + po + " already known.");
		}
		grid.remove(currentMapping);
		grid.setObjectLocation(mapping, po);
	}

	public void mapObstacle(Double2D position) {
		map(position, new Obstacle());
		
	}

	public boolean isFree(Double2D pos) {
		return mapping(pos) instanceof FreeSpace;
	}

	public Collection<Mapping> visible(Double2D position) {
		return getSurrounding(position, 1.0 * 1.12); // TODO 1.0 as a param
	}

	public Collection<Double2D> visibleUnknowns(Double2D po) {
		Set<Double2D> a = new HashSet<Double2D>();
		Collection<Mapping> visible = visible(po);
		for (Mapping mapping : visible) {
			if (mapping instanceof Unknown) { 
				a.add(grid.getObjectLocation(mapping));
			}			
		}
		return a;
	}

	public Set<Double2D> nonObstacles(Collection<Double2D> a) {
		Set<Double2D> r = new HashSet<Double2D>();
		for (Double2D position : a) {
			if (!isObstacle(position) ) {
				r .add(position);
			}
		}
		return r;
	}

	public Collection<Double2D> obstacles(Collection<Double2D> potentialObstacles) {
		 potentialObstacles.removeAll(nonObstacles(potentialObstacles));
		 return potentialObstacles;
	}

	

	

}
